/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class LineTrackerTest1 extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    DigitalInput LTrackerL = new DigitalInput(1);
    DigitalInput LTrackerM = new DigitalInput(2);
    DigitalInput LTrackerR = new DigitalInput(3);
    
    public void robotInit() {

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        

    }
    
    

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        getLineTracker();
    }
    
    public void getLineTracker() {
        int Left = LTrackerL.get() ? (int) 100: (int) 0;
        int Middle = LTrackerM.get() ? (int) 10: (int) 0;
        int Right = LTrackerR.get() ? (int) 1: (int) 0;
        
        /**
         * Gets the value of each line tracker and assigns each input an integer
         * value in the form "LMR" (Left/Middle/Right). A one in each decimal
         * place denotes the "true" value of that line tracker.
         */

        int LineTotal = Left + Middle + Right;

        switch(LineTotal) {
            case 000:
                System.out.println("THIS IS VERY BAD");
                break;
            case 001:
                System.out.println("Line is to the RIGHT");
                break;
            case 010:
                System.out.println("Driving on the line");
                break;
            case 011:
                System.out.println("MIDDLE and RIGHT detected");
                break;
            case 100:
                System.out.println("Line is to the LEFT");
                break;
            case 101:
                System.out.println("Approaching FORK");
                break;
            case 110:
                System.out.println("LEFT and MIDDLE detected");
                break;
            case 111:
                System.out.println("END OF THE LINE");
                break;

        }
    }
}
